package box2d_cgo

import (
	"math"
)

func B2IsValid(x float32) bool {
	return true
}

func B2InvSqrt(x float32) float32 {
	var xhalf float32 = 0.5 * x // get bits for floating VALUE
	i := math.Float32bits(x)    // gives initial guess y0
	i = 0x5f375a86 - (i >> 1)   // convert bits BACK to float
	x = math.Float32frombits(i) // Newton step, repeating increases accuracy
	x = x * (1.5 - xhalf*x*x)
	x = x * (1.5 - xhalf*x*x)
	x = x * (1.5 - xhalf*x*x)
	return x
}

func B2Sqrt(x float32) float32 {
	return float32(math.Sqrt(float64(x)))
}

func b2Atan2(y, x float32) float32 {
	return float32(math.Atan2(float64(y), float64(x)))
}

type B2Vec2 struct {
	X float32
	Y float32
}

func NewB2Vec2(x, y float32) *B2Vec2 {
	retVec := &B2Vec2{x, y}
	return retVec
}

func (b *B2Vec2) SetZero() {
	b.X = 0
	b.Y = 0
}

func (b *B2Vec2) Set(x, y float32) {
	b.X = x
	b.Y = y
}

//-
func (b *B2Vec2) Sub(other *B2Vec2) *B2Vec2 {
	var retVec *B2Vec2 = &B2Vec2{b.X, b.Y}
	retVec.Set(-other.X, -other.Y)
	return retVec
}

func (this *B2Vec2) GetIndex(i int) float32 {
	if i == 0 {
		return this.X
	} else {
		return this.Y
	}
}
func (this *B2Vec2) SetIndex(i int, v float32) {
	if i == 0 {
		this.X = v
	} else {
		this.Y = v
	}
}

//==
func (this *B2Vec2) Equal(other *B2Vec2) bool {
	if this.X == other.X && this.Y == other.Y {
		return true
	}
	return false
}

//!=
func (this *B2Vec2) NotEqual(other *B2Vec2) bool {
	if this.X != other.X || this.Y != other.Y {
		return true
	}
	return false
}

//+
func (b *B2Vec2) Add(other *B2Vec2) *B2Vec2 {
	retVec := &B2Vec2{b.X, b.Y}
	retVec.Add2Me(other)
	return retVec
}

func (b *B2Vec2) Add2Me(other *B2Vec2) {
	b.X += other.X
	b.Y += other.Y
}

func (b *B2Vec2) Sub2Me(other *B2Vec2) {
	b.X -= other.X
	b.Y -= other.Y
}

func (b *B2Vec2) Multiply2Me(a float32) {
	b.X *= a
	b.Y *= a
}

func (b *B2Vec2) Multiply(a float32) *B2Vec2 {
	retVec := &B2Vec2{b.X, b.Y}
	retVec.Multiply2Me(a)
	return retVec
}

func (b *B2Vec2) Length() float32 {
	return B2Sqrt(b.X*b.X + b.Y*b.Y)
}

func (b *B2Vec2) LengthSquared() float32 {
	return b.X*b.X + b.Y*b.Y
}

func (b *B2Vec2) Normalize() float32 {
	length := b.Length()
	if length < b2_epsilon {
		return 0.0
	}
	invLength := 1.0 / length
	b.X *= invLength
	b.Y *= invLength
	return length
}

func (b *B2Vec2) IsValid() bool {
	return B2IsValid(b.X) && B2IsValid(b.Y)
}

func (b *B2Vec2) Skew() *B2Vec2 {
	return NewB2Vec2(-b.Y, b.X)
}

type B2Vec3 struct {
	X float32
	Y float32
	Z float32
}

func NewB2Vec3(x, y, z float32) *B2Vec3 {
	retVec3 := &B2Vec3{x, y, z}
	return retVec3
}
func (b *B2Vec3) SetZero() {
	b.X = 0
	b.Y = 0
	b.Z = 0
}

func (b *B2Vec3) Set(x, y, z float32) {
	b.X = x
	b.Y = y
	b.Z = z
}
func (b *B2Vec3) Sub(other *B2Vec3) {
	retVec3 := &B2Vec3{b.X, b.Y, b.Z}
	retVec3.Set(-other.X, -other.Y, -other.Z)
}
func (b *B2Vec3) Add2Me(other *B2Vec3) {
	b.X += other.X
	b.Y += other.Y
	b.Z += other.Z
}
func (b *B2Vec3) Sub2Me(other *B2Vec3) {
	b.X -= other.X
	b.Y -= other.Y
	b.Z -= other.Z
}

func (b *B2Vec3) Multiply2Me(s float32) {
	b.X *= s
	b.Y *= s
	b.Z *= s
}
func (b *B2Vec3) Multiply(s float32) *B2Vec3 {
	retVec := &B2Vec3{b.X, b.Y, b.Z}
	retVec.Multiply2Me(s)
	return retVec
}
func (b *B2Vec3) Add(other *B2Vec3) *B2Vec3 {
	retVec := &B2Vec3{b.X, b.Y, b.Z}
	retVec.Add2Me(other)
	return retVec
}

type B2Mat22 struct {
	EX B2Vec2
	EY B2Vec2
}

func NewB2Mat22(a11, a12, a21, a22 float32) *B2Mat22 {
	retB2Mat22 := &B2Mat22{}
	retB2Mat22.EX.X = a11
	retB2Mat22.EX.Y = a21
	retB2Mat22.EY.X = a12
	retB2Mat22.EY.Y = a22
	return retB2Mat22
}
func NewB2Mat22Vec(c1, c2 B2Vec2) *B2Mat22 {
	retB2Mat22 := &B2Mat22{c1, c2}
	return retB2Mat22
}

func (b *B2Mat22) Set(c1, c2 B2Vec2) {
	b.EX = c1
	b.EY = c2
}

func (b *B2Mat22) SetIdentity() {
	b.EX.X = 1.0
	b.EY.X = 0.0
	b.EX.Y = 0.0
	b.EY.Y = 1.0
}
func (b *B2Mat22) SetZero() {
	b.EX.X = 0
	b.EX.Y = 0
	b.EY.X = 0
	b.EY.Y = 0
}

func (this *B2Mat22) GetInverse() *B2Mat22 {
	a := this.EX.X
	b := this.EY.X
	c := this.EX.Y
	d := this.EY.Y
	var B *B2Mat22 = &B2Mat22{}
	det := a*d - b*c
	if det != 0 {
		det = 1.0 / det
	}
	B.EX.X = det * d
	B.EY.X = -det * b
	B.EX.Y = -det * c
	B.EY.Y = det * a
	return B
}

func (this *B2Mat22) Solve(b *B2Vec2) *B2Vec2 {
	a11 := this.EX.X
	a12 := this.EY.X
	a21 := this.EX.Y
	a22 := this.EY.Y
	det := a11*a22 - a12*a21
	if det != 0 {
		det = 1.0 / det
	}
	x := &B2Vec2{}
	x.X = det * (a22*b.X - a12*b.Y)
	x.Y = det * (a11*b.Y - a21*b.X)
	return x
}

type B2Mat33 struct {
	EX B2Vec3
	EY B2Vec3
	EZ B2Vec3
}

func NewB2Mat33(c1, c2, c3 B2Vec3) *B2Mat33 {
	retB2Mat33 := &B2Mat33{c1, c2, c3}
	return retB2Mat33
}
func (this *B2Mat33) SetZero() {
	this.EX.SetZero()
	this.EY.SetZero()
	this.EZ.SetZero()
}

func (this *B2Mat33) Slove33(b *B2Vec3) *B2Vec3 {
	det := B2Dot(&this.EX, B2Cross(&this.EY, &this.EZ))
	if det != 0 {
		det = 1.0 / det
	}
	var x B2Vec3
	x.X = det * B2Dot(b, B2Cross(&this.EY, &this.EZ))
	x.Y = det * B2Dot(&this.EX, B2Cross(b, &this.EZ))
	x.Z = det * B2Dot(&this.EX, B2Cross(&this.EY, b))
	return &x
}
func (this *B2Mat33) Solve22(b *B2Vec2) *B2Vec2 {
	a11 := this.EX.X
	a12 := this.EY.X
	a21 := this.EX.Y
	a22 := this.EY.Y
	det := a11*a22 - a12*a21
	if det != 0 {
		det = 1.0 / det
	}
	var x B2Vec2
	x.X = det * (a22*b.X - a12*b.Y)
	x.Y = det * (a11*b.Y - a21*b.X)
	return &x
}

func (this *B2Mat33) GetInverse22(M *B2Mat33) {
	a := this.EX.X
	b := this.EY.X
	c := this.EX.Y
	d := this.EY.Y
	det := a*d - b*c
	if det != 0 {
		det = 1.0 / det
	}
	M.EX.X = det * d
	M.EY.X = -det * b
	M.EX.Z = 0
	M.EX.Y = -det * c
	M.EY.Y = det * a
	M.EY.Z = 0
	M.EZ.X = 0
	M.EZ.Y = 0
	M.EZ.Z = 0
}
func (this *B2Mat33) GetSymInverse33(M *B2Mat33) {
	det := B2Dot(&this.EX, B2Cross(&this.EY, &this.EZ))
	if det != 0 {
		det = 1.0 / det
	}
	a11 := this.EX.X
	a12 := this.EY.X
	a13 := this.EZ.X
	a22 := this.EY.Y
	a23 := this.EZ.Y
	a33 := this.EZ.Z
	M.EX.X = det * (a22*a33 - a23*a23)
	M.EX.Y = det * (a13*a23 - a12*a33)
	M.EX.Z = det * (a12*a23 - a13*a22)

	M.EY.X = M.EX.Y
	M.EY.Y = det * (a11*a33 - a13*a13)
	M.EY.Z = det * (a13*a12 - a11*a23)

	M.EZ.X = M.EX.Z
	M.EZ.Y = M.EY.Z
	M.EZ.Z = det * (a11*a22 - a12*a12)
}

type B2Rot struct {
	S float32
	C float32
}

func NewB2Rot(angle float32) *B2Rot {
	retRot := &B2Rot{}
	retRot.S = Sinf(angle)
	retRot.C = Cosf(angle)
	return retRot
}
func (this *B2Rot) Set(angle float32) {
	this.S = Sinf(angle)
	this.C = Cosf(angle)
}
func (this *B2Rot) SetIdentity() {
	this.S = 0.0
	this.C = 1.0
}
func (rot *B2Rot) GetAngle() float32 {
	return b2Atan2(rot.S, rot.C)
}
func (rot *B2Rot) GetXAxis() *B2Vec2 {
	return NewB2Vec2(rot.C, rot.S)
}
func (rot *B2Rot) GetYAxis() *B2Vec2 {
	return NewB2Vec2(-rot.S, rot.C)
}

type B2Transform struct {
	P B2Vec2
	Q B2Rot
}

func NewB2Transform(pos *B2Vec2, rot *B2Rot) *B2Transform {
	retTrans := &B2Transform{*pos, *rot}
	return retTrans
}
func (trans *B2Transform) SetIdentity() {
	trans.P.SetZero()
	trans.Q.SetIdentity()
}
func (trans *B2Transform) Set(pos *B2Vec2, angle float32) {
	trans.P = *pos
	trans.Q.Set(angle)
}

type B2Sweep struct {
	LocalCenter B2Vec2
	C0, C       B2Vec2
	A0, A       float32
	Alpha0      float32
}

func (this *B2Sweep) GetTransform(xf *B2Transform, beta float32) {
	xf.P = *(this.C0.Multiply(1.0 - beta).Add(this.C.Multiply(beta)))
	angle := (1.0-beta)*this.A0 + beta*this.A
	xf.Q.Set(angle)
	xf.P = *B2Mul(&xf.Q, &this.LocalCenter)
}

func (this *B2Sweep) Advance(alpha float32) {
	if this.Alpha0 < 1 {
		panic("box2d B2Sweep Advance error")
	}
	beta := (alpha - this.Alpha0) / (1.0 - this.Alpha0)
	this.C0.Add2Me(this.C.Sub(&this.C0).Multiply(beta))
	this.A0 = beta * (this.A - this.A0)
	this.Alpha0 = alpha
}

func (this *B2Sweep) Normalize() {
	twoPi := float32(2.0 * b2_pi)
	d := twoPi * Floorf(this.A0/twoPi)
	this.A0 -= d
	this.A -= d
}

var B2Vec2_zero B2Vec2

func B2Dot(a, b *B2Vec3) float32 {
	return a.X*b.X + a.Y*b.Y + a.Z*b.Z
}
func B2DotV2(a, b *B2Vec2) float32 {
	return a.X*b.X + a.Y*b.Y
}

func B2Cross(a, b *B2Vec3) *B2Vec3 {
	return &B2Vec3{a.Y*b.Z - a.Z*b.Y, a.Z*b.X - a.X*b.Z, a.X*b.Y - a.Y*b.X}
}
func B2Cross1f(a *B2Vec2, s float32) *B2Vec2 {
	return NewB2Vec2(s*a.Y, -s*a.X)
}
func B2Cross0f(s float32, a *B2Vec2) *B2Vec2 {
	return NewB2Vec2(-s*a.Y, s*a.X)
}

func Sinf(f float32) float32 {
	return float32(math.Sin(float64(f)))
}
func Cosf(f float32) float32 {
	return float32(math.Cos(float64(f)))
}
func Floorf(f float32) float32 {
	return float32(math.Floor(float64(f)))
}

func B2Mul_MV(A *B2Mat22, v *B2Vec2) *B2Vec2 {
	return NewB2Vec2(A.EX.X*v.X+A.EY.X*v.Y, A.EX.Y*v.X+A.EY.Y*v.Y)
}

func B2MulT_MV(A *B2Mat22, v *B2Vec2) *B2Vec2 {
	return NewB2Vec2(B2DotV2(v, &A.EX), B2DotV2(v, &A.EY))
}

func B2Mul(q *B2Rot, v *B2Vec2) *B2Vec2 {
	return NewB2Vec2(q.C*v.X-q.S*v.Y, q.S*v.X+q.C*v.Y)
}

func B2Distance(a, b *B2Vec2) float32 {
	c := a.Sub(b)
	return c.Length()
}
func B2DistanceSquared(a, b *B2Vec2) float32 {
	c := a.Sub(b)
	return B2DotV2(c, c)
}

func B2Mul_MM(A, B *B2Mat22) *B2Mat22 {
	return NewB2Mat22Vec(*B2Mul_MV(A, &B.EX), *B2Mul_MV(A, &B.EY))
}
func B2MulT_MM(A, B *B2Mat22) *B2Mat22 {
	c1 := NewB2Vec2(B2DotV2(&A.EX, &B.EX), B2DotV2(&A.EY, &B.EX))
	c2 := NewB2Vec2(B2DotV2(&A.EX, &B.EY), B2DotV2(&A.EY, &B.EY))
	return NewB2Mat22Vec(*c1, *c2)
}

func B2MulV3_MV(A *B2Mat33, v *B2Vec3) *B2Vec3 {
	return A.EX.Multiply(v.X).Add(A.EY.Multiply(v.Y)).Add(A.EZ.Multiply(v.Z))
}

func B2Mul22_M3V2(A *B2Mat33, v *B2Vec2) *B2Vec2 {
	return NewB2Vec2(A.EX.X*v.X+A.EY.X*v.Y, A.EX.Y*v.X+A.EY.Y*v.Y)
}

func B2Mul_R(q, r *B2Rot) *B2Rot {
	var qr B2Rot
	qr.S = q.S*r.C + q.C*r.S
	qr.C = q.C*r.C - q.S*r.S
	return &qr
}
func B2MulT_R(q, r *B2Rot) *B2Rot {
	var qr B2Rot
	qr.S = q.S*r.S - q.S*r.C
	qr.C = q.C*r.C + q.S*r.S
	return &qr
}

func B2Mul_RV(q *B2Rot, v *B2Vec2) *B2Vec2 {
	return NewB2Vec2(q.C*v.X-q.S*v.Y, q.S*v.X+q.C*v.Y)
}

func B2MulT_RV(q *B2Rot, v *B2Vec2) *B2Vec2 {
	return NewB2Vec2(q.C*v.X+q.S*v.Y, -q.S*v.X+q.C*v.Y)
}

func B2Mul_TV(T *B2Transform, v *B2Vec2) *B2Vec2 {
	x := (T.Q.C*v.X - T.Q.S*v.Y) + T.P.X
	y := (T.Q.S*v.X + T.Q.C*v.Y) + T.P.Y
	return NewB2Vec2(x, y)
}

func B2MulT_TV(T *B2Transform, v *B2Vec2) *B2Vec2 {
	px := v.X - T.P.X
	py := v.Y - T.P.Y
	x := (T.Q.C*px + T.Q.S*py)
	y := (-T.Q.S*px + T.Q.C*py)
	return NewB2Vec2(x, y)
}
func B2Mul_TT(A *B2Transform, B *B2Transform) *B2Transform {
	var C B2Transform
	C.Q = *B2Mul_R(&A.Q, &B.Q)
	C.P = *B2Mul_RV(&A.Q, &B.P).Add(&A.P)
	return &C
}
func B2MulT_TT(A *B2Transform, B *B2Transform) *B2Transform {
	var C B2Transform
	C.Q = *B2MulT_R(&A.Q, &B.Q)
	C.P = *B2MulT_RV(&A.Q, B.P.Sub(&A.P))
	return &C
}
func B2AbsFloat32(a float32) float32 {
	if a > 0 {
		return a
	} else {
		return -a
	}
}
func B2AbsV2(a *B2Vec2) *B2Vec2 {
	return NewB2Vec2(B2AbsFloat32(a.X), B2AbsFloat32(a.Y))
}

func B2AbsMat22(A *B2Mat22) *B2Mat22 {
	return NewB2Mat22Vec(*B2AbsV2(&A.EX), *B2AbsV2(&A.EY))
}
func MinFloat32(a, b float32) float32 {
	if a < b {
		return a
	} else {
		return b
	}
}

func B2MinV2(a, b *B2Vec2) *B2Vec2 {
	return NewB2Vec2(MinFloat32(a.X, b.X), MinFloat32(a.Y, b.Y))
}

func MaxFloat32(a, b float32) float32 {
	if a > b {
		return a
	} else {
		return b
	}
}
func B2MaxV2(a, b *B2Vec2) *B2Vec2 {
	return NewB2Vec2(MaxFloat32(a.X, b.X), MaxFloat32(a.Y, b.Y))
}
func ClampFloat32(a, low, high float32) float32 {
	return MaxFloat32(low, MinFloat32(a, high))
}

func B2ClampV2(a, low, high *B2Vec2) *B2Vec2 {
	return B2MaxV2(low, B2MinV2(a, high))
}
func B2NextPowerOfTwo(x uint32) uint32 {
	x |= (x >> 1)
	x |= (x >> 2)
	x |= (x >> 4)
	x |= (x >> 8)
	x |= (x >> 16)
	return x + 1
}

func B2IsPowerOfTwo(x uint32) bool {
	return x > 0 && (x&(x-1)) == 0
}

func B2SweepFloat32(a, b *float32) {
	tmp := *a
	*a = *b
	*b = tmp
}
